/*
 * Servo.cpp
 *
 *  Created on: Sep 27, 2012
 *      Author: bouchier
 */

#include <iostream>
#include <qeservo.h>
#include "servo.h"

/*
 * Vex servo moves 100 degrees from Set(1) to Set(255). i.e. input range is -50 - +50
 * Vex 393 motor free speed is 100rpm => 1.6rps. With 5" wheels this is 26"/sec unloaded.
 * Suggest setting input range to -20 - +20 ips
 *
 * Increasing motor value makes a servo turn clockwise looking at shaft.
 * Increasing motor value makes a motor turn counterclockwise looking at shaft
 */

/*
 * @param num Motor index; 0 = motor 1
 * @param setZero the value to apply to set the motor to stopped or servo to midrange
 * @param scale should be set to 127/(max value). Input is multiplied by scale & added to zero
 * @param setLimit The value added & subtracted from setZero which results in the limits placed on
 * the value applied to the motor
 */
ServoMotor::ServoMotor(int num, int setZero, float setScale, int setLimit, bool reversedIn) : servo(CQEServo::GetRef())
{
	motorNum = num;
	zero = setZero;
	scale = setScale;
	limit = setLimit;
	reversed = reversedIn;
}

int ServoMotor::set(float rqValue)
{
	int motorVal;
	int error = 0;

	if (reversed)
		rqValue = 0 - rqValue;		// change polarity of the request if the motor is reversed
	motorVal = zero + (int)((scale * rqValue) + 0.5);
	if (motorVal < (zero-limit)) {
		motorVal = zero - limit;
		error = -1;
	}
	if (motorVal > (zero + limit)) {
		motorVal = zero + limit;
		error = -2;
	}
	value = rqValue;
	//printf("setting servo/motor %d with request %0.2f to %d\n", motorNum, value, motorVal);
	servo.SetCommand(motorNum, motorVal);
	return error;
}

float ServoMotor::getRq()
{
	return value;
}
